/*
 * Triange.h
 *
 * Created 5/16/2009 By Johnny Huynh
 *
 * Version 00.00.03 7/4/2009
 *
 * Copyright Information:
 * All content copyright © 2009 Johnny Huynh. All rights reserved.
 */
 
 // Triangle.h contains all the primitive triangle functions

 #ifndef TRIANGLE_H
 #define TRIANGLE_H
 

 
 template< typename TYPENAME > class Triangle;
 
 typedef Triangle<GLfloat> Trianglef;
 typedef Triangle<GLdouble> Triangled;
 
 #include "global.h"
 #include "Vector3.h"
 #include "OpenGL_Headers.h"
 #include "GL_Render_Shape.h"
 #include <math.h>
 #include <vector>
 
 #include "Math3D.h"
 
 template< typename TYPENAME >
 class Triangle
 {
 // Data Members
 protected:
    Vector3<TYPENAME> a, b, c;  // the 3 endpoints a, b, and c of the triangle
 
 // Local Functions
 public:
    Triangle( const TYPENAME& ax, const TYPENAME& ay, const TYPENAME& az,
              const TYPENAME& bx, const TYPENAME& by, const TYPENAME& bz,
              const TYPENAME& cx, const TYPENAME& cy, const TYPENAME& cz );
    Triangle( const Vector3<TYPENAME>& a, const Vector3<TYPENAME>& b, const Vector3<TYPENAME>& c );
    Triangle( const Triangle<TYPENAME>& t );
    ~Triangle();
    inline Triangle<TYPENAME>& operator=( const Triangle<TYPENAME>& t );
    inline const Vector3<TYPENAME>& getEndPointA() const;
    inline const Vector3<TYPENAME>& getEndPointB() const;
    inline const Vector3<TYPENAME>& getEndPointC() const;
    virtual inline const TYPENAME getArea() const;
    virtual inline const Vector3<TYPENAME> getCenter() const;
    virtual inline const Vector3<TYPENAME> getNormal() const;
    
 // Friend Functions
    template <typename TYPENAME> friend inline TYPENAME area( const Triangle<TYPENAME>& t );
    template <typename TYPENAME> friend inline Vector3<TYPENAME> center( const Triangle<TYPENAME>& t );
    template <typename TYPENAME> friend inline Vector3<TYPENAME> normal( const Triangle<TYPENAME>& t );
    template <typename TYPENAME> friend inline Vector3<TYPENAME> center( std::vector< const Triangle<TYPENAME>* >& triangles );
    template <typename TYPENAME> friend inline Vector3<TYPENAME> center( const std::vector< const Triangle<TYPENAME> >& triangles );
    template <typename TYPENAME> friend inline bool collide( const Triangle<TYPENAME>& t, const Triangle<TYPENAME>& u );
    template <typename TYPENAME> friend inline bool collideSeparatingAxisTheorem( const Triangle<TYPENAME>& t, const Triangle<TYPENAME>& u );
    template <typename TYPENAME> friend inline bool collideSeparatingAxis( const Triangle<TYPENAME>& t, const Triangle<TYPENAME>& u, const Vector3<TYPENAME>& axis );
 #if IMPLEMENT_INTERPOLATION == 1
    template <typename TYPENAME> friend inline TYPENAME collisionExtent( const Triangle<TYPENAME>& t, 
                            const Triangle<TYPENAME>& u, Vector3<TYPENAME>& t_displacement, 
                            Vector3<TYPENAME>& u_displacement );
    template <typename TYPENAME> friend inline TYPENAME collisionExtent( const Triangle<TYPENAME>& t, 
                            const Triangle<TYPENAME>& u, const Vector3<TYPENAME>& t_displacement, 
                            const Vector3<TYPENAME>& u_displacement );
    template <typename TYPENAME> friend inline TYPENAME collisionExtent( const Triangle<TYPENAME>& t, 
                            const Triangle<TYPENAME>& u, const Vector3<TYPENAME>& t_displacement, 
                            const Vector3<TYPENAME>& u_displacement, const Vector3<TYPENAME>& axis );
 #endif // IMPLEMENT_INTERPOLATION == 1
    //template <typename TYPENAME> friend inline void findMinMaxProjectionExtent( const Triangle<TYPENAME>& triangle, 
    //                                                          const Vector3<TYPENAME>& axis, TYPENAME& min, TYPENAME& max );
    template <typename TYPENAME> friend inline bool PointTriangleIntersects( const Vector3<TYPENAME>& point, 
                                                    const Triangle<TYPENAME>& triangle, const TYPENAME& triangleArea );
    template <typename TYPENAME> friend inline bool LineTriangleIntersects( 
                                                    const Vector3<TYPENAME>& lineOrigin, const Vector3<TYPENAME>& line, 
                                                    const Triangle<TYPENAME>& triangle, const TYPENAME& triangleArea );
    template <typename TYPENAME> friend inline bool RayTriangleIntersects( 
                                                    const Vector3<TYPENAME>& rayOrigin, const Vector3<TYPENAME>& rayDirection,
                                                    const Triangle<TYPENAME>& triangle, const TYPENAME& triangleArea );
    template <typename TYPENAME> friend inline void print( const Triangle<TYPENAME>& t );
    template <typename TYPENAME> friend inline void render( const Triangle<TYPENAME>& t );
    
 };
 
 /** LOCAL FUNCTIONS **/
 
 /**
  * Constructor
  * The constructor takes in the three positions that define the triangle.
  *
  * @param (const TYPENAME&) ax, (const TYPENAME&) ay, (const TYPENAME&) az
  * @param (const TYPENAME&) bx, (const TYPENAME&) by, (const TYPENAME&) bz
  * @param (const TYPENAME&) cx, (const TYPENAME&) cy, (const TYPENAME&) cz
  */
 template< typename TYPENAME >
 Triangle<TYPENAME>::Triangle( const TYPENAME& ax, const TYPENAME& ay, const TYPENAME& az,
                               const TYPENAME& bx, const TYPENAME& by, const TYPENAME& bz,
                               const TYPENAME& cx, const TYPENAME& cy, const TYPENAME& cz )
                              : a(Vector3<TYPENAME>( ax, ay, az )), 
                                b(Vector3<TYPENAME>( bx, by, bz )), 
                                c(Vector3<TYPENAME>( cx, cy, cz ))
 {

 }
 
 /**
  * Alternative Constructor
  * The constructor takes in the three positions that defines the triangle.
  *
  * @param (const Vector3<TYPENAME>&) a
  * @param (const Vector3<TYPENAME>&) b
  * @param (const Vector3<TYPENAME>&) c
  */
 template< typename TYPENAME >
 Triangle<TYPENAME>::Triangle( const Vector3<TYPENAME>& a, const Vector3<TYPENAME>& b, const Vector3<TYPENAME>& c )
                                : a(a), b(b), c(c)
 {

 }

 /**
  * Alternative Constructor
  * The constructor copies the content of the specified triangle.
  *
  * @param (const Triangle<TYPENAME>&) t
  */
 template< typename TYPENAME >
 Triangle<TYPENAME>::Triangle( const Triangle<TYPENAME>& t )
 {
    memcpy( this, &t, sizeof(Triangle<TYPENAME>) );
 }
 
 /**
  * Destructor
  */
 template< typename TYPENAME >
 Triangle<TYPENAME>::~Triangle()
 {
 
 }
 
 /**
  * operator=() copies the content of the specified triangle to this triangle.
  *
  * @param (const Triangle<TYPENAME>&) t
  */
 template< typename TYPENAME >
 inline Triangle<TYPENAME>& Triangle<TYPENAME>::operator=( const Triangle<TYPENAME>& t )
 {
    memcpy( this, &t, sizeof(Triangle<TYPENAME>) );
 }
 
 /**
  * getEndPointA() returns the end point a of this triangle.
  *
  * @return const Vector3<TYPENAME>&
  */
 template <typename TYPENAME>
 inline const Vector3<TYPENAME>& Triangle<TYPENAME>::getEndPointA() const
 {
    return a;
 }
 
 /**
  * getEndPointB() returns the end point b of this triangle.
  *
  * @return const Vector3<TYPENAME>&
  */
 template <typename TYPENAME>
 inline const Vector3<TYPENAME>& Triangle<TYPENAME>::getEndPointB() const
 {
    return b;
 }
 
 /**
  * getEndPointC() returns the end point c of this triangle.
  *
  * @return const Vector3<TYPENAME>&
  */
 template <typename TYPENAME>
 inline const Vector3<TYPENAME>& Triangle<TYPENAME>::getEndPointC() const
 {
    return c;
 }
 
 /**
  * getArea() returns the area of this triangle.
  *
  * @return const TYPENAME&
  */
 template <typename TYPENAME>
 inline const TYPENAME Triangle<TYPENAME>::getArea() const
 {
    // Take point b to point c as the base of the parallelogram
    Vector3<TYPENAME> base = c - b;
    
    // Take point b to point a as the side of the parallelogram
    Vector3<TYPENAME> side = a - b;
    
    Vector3<TYPENAME> orthogonalVector = crossProduct( base, side );
    
    // The area of the parallelogram is the magnitude of the cross product
    // The area of the triangle is half the area of the parallelogram
    return sqrt((orthogonalVector*orthogonalVector)) / 2.0f;
 }
 
 /**
  * getCenter() returns the center point of this triangle.
  *
  * @return const Vector3<TYPENAME>&
  */
 template <typename TYPENAME>
 inline const Vector3<TYPENAME> Triangle<TYPENAME>::getCenter() const
 {
    return (a + b + c) / 3.0f;
 }
 
 /**
  * getNormal() returns the normal unit vector of this triangle.
  * Assumes counter-clockwise winding 
  * (i.e. if a to b is the vector (1, 0, 0) and a to c is the vector
  * (0, 1, 0), the resulting normal vector is (0, 0, 1)).
  *
  * @return const Vector3<TYPENAME>&
  */
 template <typename TYPENAME>
 inline const Vector3<TYPENAME> Triangle<TYPENAME>::getNormal() const
 {
    // normalize( b-a x c-a )
    return normalize( crossProduct( b - a, c - a ) );
 }
 
 /** FRIEND FUNCTIONS **/
 
 /**
  * area() returns the area of the specified triangle.
  *
  * @param (const Triangle<TYPENAME>&) t
  * @return TYPENAME
  */
 template <typename TYPENAME>
 inline TYPENAME area( const Triangle<TYPENAME>& t )
 {
    return t.getArea();
    
    /*// Take point b to point c as the base of the parallelogram
    Vector3<TYPENAME> base = t.c - t.b;
    
    // Take point b to point a as the side of the parallelogram
    Vector3<TYPENAME> side = t.a - t.b;
    
    Vector3<TYPENAME> orthogonalVector = crossProduct( base, side );
    
    // The area of the parallelogram is the magnitude of the cross product
    // The area of the triangle is half the area of the parallelogram
    return sqrt((orthogonalVector*orthogonalVector)) / 2.0f;*/
    
    /*
    // take point b to point c as the base of the parallelogram and triangle
    Vector3<TYPENAME> base = t.c - t.b;
    
    // take point b to point a as the side of the parallelogram
    Vector3<TYPENAME> side = t.a - t.b;
    
    // unit vector of the base
    Vector3<TYPENAME> u = base.getgetNormal();
    
    // project the side vector onto the base unit vector
    TYPENAME s = side * u;
    
    // vector of the projection
    Vector3<TYPENAME> p = s*u;
    
    // Suppose we want to find the vector v (defining the height of the parallelogram)
    // Then,
    // a - v = b + p
    // v = a - b - p
    Vector3<TYPENAME> v = t.a - t.b - p;
    
    // The area of the parallelogram is then the magnitude of v multiplied by the magnitude of the base.
    // sqrt( base^2 ) * sqrt( v^2 )
    // = sqrt( base^2 * v^2 )
    // The area of the triangle is half the area of the parallelogram.
    // Thus, the area of the triangle is
    // sqrt( base^2 * v^2 ) / 2
    return sqrt( (base*base)*(v*v) ) / 2.0f;*/
 }
 
 /**
  * center() returns the center point of the specified triangle.
  *
  * @param (const Triangle<TYPENAME>&) t
  * @return Vector3<TYPENAME>
  */
 template <typename TYPENAME>
 inline Vector3<TYPENAME> center( const Triangle<TYPENAME>& t )
 {
    return t.getCenter();
    //return (t.a + t.b + t.c) / (TYPENAME)0x3;
 }
 
 /**
  * normal() returns the normal unit vector of the specified triangle.
  * Assumes counter-clockwise winding 
  * (i.e. if a to b is the vector (1, 0, 0) and a to c is the vector
  * (0, 1, 0), the resulting normal vector is (0, 0, 1)).
  *
  * @param (const Triangle<TYPENAME>&) t
  * @return Vector3<TYPENAME>
  */
 template <typename TYPENAME>
 inline Vector3<TYPENAME> normal( const Triangle<TYPENAME>& t )
 {
    return t.getNormal();
    // normalize( b-a x c-a )
    // return normalize( crossProduct( t.b - t.a, t.c - t.a ) );
 }
 
 /**
  * center() returns the centroid of the specified group of triangles.
  *
  * @param (const std::vector< const Triangle<TYPENAME> >&) triangles
  * @return Vector3<TYPENAME>
  */
 template <typename TYPENAME> 
 inline Vector3<TYPENAME> center( const std::vector< const Triangle<TYPENAME> >& triangles )
 {
    // n = number of triangles
    // centroid = (1 / (3n)) * sum( ith_a + ith_b + ith_c )
    Vector3<TYPENAME> centroid( 0.0f, 0.0f, 0.0f );

    std::vector< const Triangle<TYPENAME> >::const_iterator itr;
    for ( itr = triangles.begin(); itr != triangles.end(); ++itr )
	{
	    const Triangle<TYPENAME>& triangle = *itr;
	    
	    // sum( ith_a + ith_b + ith_c )
	    centroid += triangle.getEndPointA() + triangle.getEndPointB() + triangle.getEndPointC();
	}
	// centroid = (1 / (3n)) * sum( ith_a + ith_b + ith_c )
	return centroid /= 0x3*triangles.size();
 }
 
 /**
  * center() returns the centroid of the specified group of triangles.
  *
  * @param (std::vector< const Triangle<TYPENAME>* >&) triangles
  * @return Vector3<TYPENAME>
  */
 template <typename TYPENAME> 
 inline Vector3<TYPENAME> center( std::vector< const Triangle<TYPENAME>* >& triangles )
 {
    // n = number of triangles
    // centroid = (1 / (3n)) * sum( ith_a + ith_b + ith_c )
    Vector3<TYPENAME> centroid( 0.0f, 0.0f, 0.0f );

    // WARNING: Changed here after removing const
    //std::vector<const GL_Triangle<TYPENAME> * const>::const_iterator itr;
    std::vector< const Triangle<TYPENAME>* >::const_iterator itr;
    for ( itr = triangles.begin(); itr != triangles.end(); ++itr )
	{
	    const Triangle<TYPENAME> * const & triangle_Ptr = *itr;
	    
	    // sum( ith_a + ith_b + ith_c )
	    centroid += triangle_Ptr->getEndPointA() + triangle_Ptr->getEndPointB() + triangle_Ptr->getEndPointC();
	}
	// centroid = (1 / (3n)) * sum( ith_a + ith_b + ith_c )
	return centroid /= 0x3*triangles.size();
 }
 
 /**
  * collide() returns true if the two specified triangles collide; otherwise, false is returned.
  *
  * @param (const Triangle<TYPENAME>&) t
  * @param (const Triangle<TYPENAME>&) u
  * @return bool
  */
 template <typename TYPENAME>
 inline bool collide( const Triangle<TYPENAME>& t, const Triangle<TYPENAME>& u )
 {
    /*const Triangle<TYPENAME>* largerTriangle_Ptr = &t;
    const Triangle<TYPENAME>* smallerTriangle_Ptr = &u;
    
    TYPENAME largerTriangleArea = area( t );
    TYPENAME smallerTriangleArea = area( u );
    
    if ( largerTriangleArea < smallerTriangleArea )
    {
        largerTriangle_Ptr = &u;
        smallerTriangle_Ptr = &t;
        
        largerTriangleArea = smallerTriangleArea;
        //std::swap(largerTriangleArea, smallerTriangleArea);
    }*/
    return collideSeparatingAxisTheorem( t, u );
    TYPENAME tArea = area( t );
    TYPENAME uArea = area( u );

    // lines of triangle t
    // ab
    // bc
    // ca
    
    // Triangles t and u collide if any of the edges of triangle t or u intersect the triangle u or t respectively.
    
    // Edges of Triangle t and Triangle u
    
    // t.ab
    // Handle line ab of triangle t intersection with triangle u
    if ( LineTriangleIntersects( t.a, t.b - t.a, u, uArea ) )
        return true;
    
    // u.ab
    // Handle line ab of triangle u intersection with triangle t
    if ( LineTriangleIntersects( u.a, u.b - u.a, t, tArea ) )
        return true;
    
    // t.bc
    // Handle line bc of triangle t intersection with triangle u
    if ( LineTriangleIntersects( t.b, t.c - t.b, u, uArea ) )
        return true;
    
    // u.bc
    // Handle line bc of triangle u intersection with triangle t
    if ( LineTriangleIntersects( u.b, u.c - u.b, t, tArea ) )
        return true;
    
    // t.ca
    // Handle line ca of triangle t intersection with triangle u
    if ( LineTriangleIntersects( t.c, t.a - t.c, u, uArea ) )
        return true;
    
    // u.ca
    // Handle line ca of triangle u intersection with triangle t
    if ( LineTriangleIntersects( u.c, u.a - u.c, t, tArea ) )
        return true;
    
    return false;
 }
 
 /**
  * collideSeparatingAxisTheorem() returns true if the two specified triangles intersect; otherwise, false is returned.
  *
  * @param (const Triangle<TYPENAME>&) t
  * @param (const Triangle<TYPENAME>&) u
  * @return bool
  */
 template <typename TYPENAME> 
 inline bool collideSeparatingAxisTheorem( const Triangle<TYPENAME>& t, const Triangle<TYPENAME>& u )
 {
    const Vector3<TYPENAME>& t_normal( t.getNormal() );
    const Vector3<TYPENAME>& u_normal( u.getNormal() );
    
    // check if it is possible for t and u to be coplanar
    if ( t_normal == u_normal )
    {
        // if t and u are coplanar
        Vector3<TYPENAME> cp( crossProduct( crossProduct( u.getEndPointA() - t.getEndPointA(), t.getEndPointB() - t.getEndPointA() ), t_normal ) );
        
        // assume t and u are coplanar if cp is approximately (0, 0, 0)
        if ( (cp.x > -EPSILON || cp.x < EPSILON) 
                        && (cp.y > -EPSILON || cp.y < EPSILON) 
                        && (cp.z > -EPSILON || cp.z < EPSILON) )
        {
            // Assume we are now performing collision detection in the 2D plane
            // There are 6 separating axes, and they are perpendicular to each of the edges
            
            // L = t.normal x (t.b - t.a)
            if ( !collideSeparatingAxis( t, u, crossProduct( t_normal, t.getEndPointB() - t.getEndPointA() ) ) )
                return false;
            
            // L = t.normal x (t.c - t.b)
            if ( !collideSeparatingAxis( t, u, crossProduct( t_normal, t.getEndPointC() - t.getEndPointB() ) ) )
                return false;
            
            // L = t.normal x (t.a - t.c)
            if ( !collideSeparatingAxis( t, u, crossProduct( t_normal, t.getEndPointA() - t.getEndPointC() ) ) )
                return false;
            
            // L = u.normal x (u.b - u.a)
            if ( !collideSeparatingAxis( t, u, crossProduct( u_normal, u.getEndPointB() - u.getEndPointA() ) ) )
                return false;
            
            // L = u.normal x (u.c - u.b)
            if ( !collideSeparatingAxis( t, u, crossProduct( u_normal, u.getEndPointC() - u.getEndPointB() ) ) )
                return false;
            
            // L = u.normal x (u.a - u.c)
            if ( !collideSeparatingAxis( t, u, crossProduct( u_normal, u.getEndPointA() - u.getEndPointC() ) ) )
                return false;
            
            return true;
        }
    }
    
    Vector3<TYPENAME> ta( t.getEndPointB() - t.getEndPointA() );
    Vector3<TYPENAME> tb( t.getEndPointC() - t.getEndPointB() );
    Vector3<TYPENAME> tc( t.getEndPointA() - t.getEndPointC() );
    
    Vector3<TYPENAME> ua( u.getEndPointB() - u.getEndPointA() );
    Vector3<TYPENAME> ub( u.getEndPointC() - u.getEndPointB() );
    Vector3<TYPENAME> uc( u.getEndPointA() - u.getEndPointC() );
    //printf("HERE\n");
    //printf( "t: %1d\n", collideSeparatingAxis( t, u, t_normal ) );
    //printf( "u: %1d\n", collideSeparatingAxis( t, u, t_normal ) );
    //printf( "ta x ua: %1d\n", collideSeparatingAxis( t, u, crossProduct( ta, ua ) ) );
    //printf( "ta x ub: %1d\n", collideSeparatingAxis( t, u, crossProduct( ta, ub ) ) );
    //printf( "ta x uc: %1d\n", collideSeparatingAxis( t, u, crossProduct( ta, uc ) ) );
    //printf( "tb x ua: %1d\n", collideSeparatingAxis( t, u, crossProduct( tb, ua ) ) );
    //printf( "tb x ub: %1d\n", collideSeparatingAxis( t, u, crossProduct( tb, ub ) ) );
    //printf( "tb x uc: %1d\n", collideSeparatingAxis( t, u, crossProduct( tb, uc ) ) );
    //printf( "tc x ua: %1d\n", collideSeparatingAxis( t, u, crossProduct( tc, ua ) ) );
    //printf( "tc x ub: %1d\n", collideSeparatingAxis( t, u, crossProduct( tc, ub ) ) );
    //printf( "tc x uc: %1d\n", collideSeparatingAxis( t, u, crossProduct( tc, uc ) ) );
    // L = normal( t )
    if ( !collideSeparatingAxis( t, u, t_normal ) )
        return false;
    // L = normal( u )
    if ( !collideSeparatingAxis( t, u, u_normal ) )
        return false;
    // L = ta x ua
    if ( !collideSeparatingAxis( t, u, crossProduct( ta, ua ) ) )
        return false;
    // L = ta x ub
    if ( !collideSeparatingAxis( t, u, crossProduct( ta, ub ) ) )
        return false;
    // L = ta x uc
    if ( !collideSeparatingAxis( t, u, crossProduct( ta, uc ) ) )
        return false;
    // L = tb x ua
    if ( !collideSeparatingAxis( t, u, crossProduct( tb, ua ) ) )
        return false;
    // L = tb x ub
    if ( !collideSeparatingAxis( t, u, crossProduct( tb, ub ) ) )
        return false;
    // L = tb x uc
    if ( !collideSeparatingAxis( t, u, crossProduct( tb, uc ) ) )
        return false;
    // L = tc x ua
    if ( !collideSeparatingAxis( t, u, crossProduct( tc, ua ) ) )
        return false;
    // L = tc x ub
    if ( !collideSeparatingAxis( t, u, crossProduct( tc, ub ) ) )
        return false;
    // L = tc x uc
    if ( !collideSeparatingAxis( t, u, crossProduct( tc, uc ) ) )
        return false;
    
    return true;
 }
 
 /**
  * collideSeparatingAxis() returns true if the projection of the two specified triangles onto the
  * specified axis intersect; otherwise, false is returned. The axis does not have to be a unit
  * vector.
  *
  * @param (const Triangle<TYPENAME>&) t
  * @param (const Triangle<TYPENAME>&) u
  * @param (const Vector3<TYPENAME>&) axis
  * @return bool
  */
 template <typename TYPENAME> 
 inline bool collideSeparatingAxis( const Triangle<TYPENAME>& t, const Triangle<TYPENAME>& u, 
                                                                 const Vector3<TYPENAME>& axis )
 {
    /*
    TYPENAME t_min( t.getEndPointA() * axis );
    TYPENAME t_max( t_min );
    findMinMaxProjectionExtent( t.getEndPointB(), axis, t_min, t_max );
    findMinMaxProjectionExtent( t.getEndPointC(), axis, t_min, t_max );
    
    TYPENAME u_min( u.getEndPointA() * axis );
    TYPENAME u_max( u_min );
    findMinMaxProjectionExtent( u.getEndPointB(), axis, u_min, u_max );
    findMinMaxProjectionExtent( u.getEndPointC(), axis, u_min, u_max );
    
    if ( t_min > u_min )
        return t_min <= u_max;  // true if t_min is between u_min and u_max (inclusive)
    else
        return u_min <= t_max;  // true if u_min is between t_min and t_max (inclusive)
    */
    
    TYPENAME t_extent_A( t.getEndPointA() * axis );
    TYPENAME t_extent_B( t.getEndPointB() * axis );
    TYPENAME t_extent_C( t.getEndPointC() * axis );
    
    TYPENAME u_extent_A( u.getEndPointA() * axis );
    TYPENAME u_extent_B( u.getEndPointB() * axis );
    TYPENAME u_extent_C( u.getEndPointC() * axis );
    
    TYPENAME t_min( ( t_extent_A < t_extent_B ? ( t_extent_A < t_extent_C ? t_extent_A : t_extent_C ) : 
                                                ( t_extent_B < t_extent_C ? t_extent_B : t_extent_C ) ) );
    
    TYPENAME u_min( ( u_extent_A < u_extent_B ? ( u_extent_A < u_extent_C ? u_extent_A : u_extent_C ) : 
                                                ( u_extent_B < u_extent_C ? u_extent_B : u_extent_C ) ) );
    
    if ( t_min > u_min )
    {
        // true if t_min is between u_min and u_max (inclusive)
        return t_min <= ( u_extent_A > u_extent_B ? ( u_extent_A > u_extent_C ? u_extent_A : u_extent_C ) : 
                                                    ( u_extent_B > u_extent_C ? u_extent_B : u_extent_C ) );  
    }
    else // t_min <= u_min
    {
        // true if u_min is between t_min and t_max (inclusive)
        return u_min <= ( t_extent_A > t_extent_B ? ( t_extent_A > t_extent_C ? t_extent_A : t_extent_C ) : 
                                                    ( t_extent_B > t_extent_C ? t_extent_B : t_extent_C ) );  
    }
 }
 
 #if IMPLEMENT_INTERPOLATION == 1
 
 #if 0
 // optimized form, but seems to have issues
 /**
  * collisionExtent() returns the extent that the triangles need to move in the directions
  * toward their previous positions to avoid the collision; if the extent returned is less
  * than zero, then the two triangles are currently not intersecting. If the two triangles
  * are colliding, but the difference between the specified t_displacement and u_displacement 
  * is the zero vector (i.e. t_displacement == u_displacement), then the t_displacement and 
  * u_displacement will be modified to //the difference between the center of the two triangles
  * or// the negative normal vectors of triangle u and triangle t respectively.
  *
  * @param (const Triangle<TYPENAME>&) t - triangle at its current position
  * @param (const Triangle<TYPENAME>&) u - triangle at its current position
  * @param (Vector3<TYPENAME>&) t_displacement - the current position of t minus the previous position of t
  * @param (Vector3<TYPENAME>&) u_displacement - the current position of u minus the previous position of u
  * @return TYPENAME
  */
 template <typename TYPENAME> 
 inline TYPENAME collisionExtent( const Triangle<TYPENAME>& t, const Triangle<TYPENAME>& u,
                                Vector3<TYPENAME>& t_displacement, Vector3<TYPENAME>& u_displacement )
 {
    // netVelocity = t_displacement - u_displacement
    Vector3<TYPENAME> netUnitVelocity( t_displacement - u_displacement );
    TYPENAME netVelocityMagnitude( magnitude( netUnitVelocity ) );
    
    const Vector3<TYPENAME>& t_normal( t.getNormal() );
    const Vector3<TYPENAME>& u_normal( u.getNormal() );
    
    // change the displacements if the displacements are equal
    if ( netUnitVelocity.isZeroVector() )
    {
        // Center seems to work better than using normal
        /*t_displacement = u.getCenter() - t.getCenter();
        u_displacement = -t_displacement;
        if ( t_displacement.isZeroVector() )
        {*/
            t_displacement = -u_normal;
            u_displacement = -t_normal;
            
            if ( t_displacement == u_displacement || ( t_displacement * u_displacement ) > ZERO )
            {
                t_displacement.x = 0.0f;
                t_displacement.y = 0.0f;
                t_displacement.z = 0.0f;
                netUnitVelocity = normal( u_displacement ); // or -u_displacement, does not matter
                                  // u_displacement is already normalized, but just incase
            }
            else
                netUnitVelocity = normal( t_displacement - u_displacement );
        /*}
        else
            netUnitVelocity = normal( t_displacement - u_displacement );*/
    }
    else
        netUnitVelocity = normal( netUnitVelocity );
    
    // min_overlapping_length in the direction of the net velocity
    // overlapping_length / | separating_axis * netVelocity |
    TYPENAME min_overlapping_length( std::numeric_limits<float>::max() );
    
    // check if it is possible for t and u to be coplanar
    if ( t_normal == u_normal )
    {
        // if t and u are coplanar
        Vector3<TYPENAME> cp( crossProduct( crossProduct( u.getEndPointA() - t.getEndPointA(), t.getEndPointB() - t.getEndPointA() ), t_normal ) );
        
        // assume t and u are coplanar if cp is approximately (0, 0, 0)
        if ( (cp.x > -EPSILON || cp.x < EPSILON) 
                        && (cp.y > -EPSILON || cp.y < EPSILON) 
                        && (cp.z > -EPSILON || cp.z < EPSILON) )
        {
            // Assume we are now performing collision detection in the 2D plane
            // There are 6 separating axes, and they are perpendicular to each of the edges
            
            Vector3<TYPENAME> L( normal( crossProduct( t_normal, t.getEndPointB() - t.getEndPointA() ) ) );
            TYPENAME overlapping_length( collisionExtent( t, u, t_displacement, u_displacement, L ) );
            
            // Case 1:
            // L = t.normal x (t.b - t.a)
            if ( overlapping_length >= 0.0f )
            {
                // Assumes use of netUnitVelocity instead of netVelocity
                overlapping_length /= fabs(netUnitVelocity * L);
                //if ( overlapping_length < min_overlapping_length )
                    min_overlapping_length = overlapping_length;
            }
            else
                return -1.0f;
            
            // Case 2:
            // L = t.normal x (t.c - t.b)
            L = normal( crossProduct( t_normal, t.getEndPointC() - t.getEndPointB() ) );
            overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
            if ( overlapping_length >= 0.0f )
            {
                // Assumes use of netUnitVelocity instead of netVelocity
                if ( overlapping_length < min_overlapping_length )
                {
                    overlapping_length /= fabs(netUnitVelocity * L);
                    if ( overlapping_length < min_overlapping_length )
                        min_overlapping_length = overlapping_length;
                }
            }
            else
                return -1.0f;
            
            // Case 3:
            // L = t.normal x (t.a - t.c)
            L = normal( crossProduct( t_normal, t.getEndPointA() - t.getEndPointC() ) );
            overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
            if ( overlapping_length >= 0.0f )
            {
                // Assumes use of netUnitVelocity instead of netVelocity
                if ( overlapping_length < min_overlapping_length )
                {
                    overlapping_length /= fabs(netUnitVelocity * L);
                    if ( overlapping_length < min_overlapping_length )
                        min_overlapping_length = overlapping_length;
                }
            }
            else
                return -1.0f;
            
            // Case 4:
            // L = u.normal x (u.b - u.a)
            L = normal( crossProduct( u_normal, u.getEndPointB() - u.getEndPointA() ) );
            overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
            if ( overlapping_length >= 0.0f )
            {
                // Assumes use of netUnitVelocity instead of netVelocity
                if ( overlapping_length < min_overlapping_length )
                {
                    overlapping_length /= fabs(netUnitVelocity * L);
                    if ( overlapping_length < min_overlapping_length )
                        min_overlapping_length = overlapping_length;
                }
            }
            else
                return -1.0f;
            
            // Case 5:
            // L = u.normal x (u.c - u.b)
            L = normal( crossProduct( u_normal, u.getEndPointC() - u.getEndPointB() ) );
            overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
            if ( overlapping_length >= 0.0f )
            {
                // Assumes use of netUnitVelocity instead of netVelocity
                if ( overlapping_length < min_overlapping_length )
                {
                    overlapping_length /= fabs(netUnitVelocity * L);
                    if ( overlapping_length < min_overlapping_length )
                        min_overlapping_length = overlapping_length;
                }
            }
            else
                return -1.0f;
            
            // Case 6:
            // L = u.normal x (u.a - u.c)
            L = normal( crossProduct( u_normal, u.getEndPointA() - u.getEndPointC() ) );
            overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
            if ( overlapping_length >= 0.0f )
            {
                // Assumes use of netUnitVelocity instead of netVelocity
                if ( overlapping_length < min_overlapping_length )
                {
                    overlapping_length /= fabs(netUnitVelocity * L);
                    if ( overlapping_length < min_overlapping_length )
                        min_overlapping_length = overlapping_length;
                }
            }
            else
                return -1.0f;
            
            return min_overlapping_length / netVelocityMagnitude;
        }
    }
    
    Vector3<TYPENAME> ta( t.getEndPointB() - t.getEndPointA() );
    Vector3<TYPENAME> tb( t.getEndPointC() - t.getEndPointB() );
    Vector3<TYPENAME> tc( t.getEndPointA() - t.getEndPointC() );
    
    Vector3<TYPENAME> ua( u.getEndPointB() - u.getEndPointA() );
    Vector3<TYPENAME> ub( u.getEndPointC() - u.getEndPointB() );
    Vector3<TYPENAME> uc( u.getEndPointA() - u.getEndPointC() );
    
    TYPENAME overlapping_length( collisionExtent( t, u, t_displacement, u_displacement, t_normal ) );
    
    // Case 1:
    // L = normal( t )
    if ( overlapping_length >= 0.0f )
    {
        overlapping_length /= fabs(netUnitVelocity * t_normal);
        //if ( overlapping_length < min_overlapping_length )
            min_overlapping_length = overlapping_length;
    }
    else
        return -1.0f;
    
    // Case 2:
    // L = normal( u )
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, u_normal );
    if ( overlapping_length >= 0.0f )
    {
        // Assumes use of netUnitVelocity instead of netVelocity
        if ( overlapping_length < min_overlapping_length )
        {
            overlapping_length /= fabs(netUnitVelocity * u_normal);
            if ( overlapping_length < min_overlapping_length )
                min_overlapping_length = overlapping_length;
        }
    }
    else
        return -1.0f;
    
    // Case 3:
    // L = ta x ua
    Vector3<TYPENAME> L( normal( crossProduct( ta, ua ) ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        // Assumes use of netUnitVelocity instead of netVelocity
        if ( overlapping_length < min_overlapping_length )
        {
            overlapping_length /= fabs(netUnitVelocity * L);
            if ( overlapping_length < min_overlapping_length )
                min_overlapping_length = overlapping_length;
        }
    }
    else
        return -1.0f;
    
    // Case 4:
    // L = ta x ub
    L = normal( crossProduct( ta, ub ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        // Assumes use of netUnitVelocity instead of netVelocity
        if ( overlapping_length < min_overlapping_length )
        {
            overlapping_length /= fabs(netUnitVelocity * L);
            if ( overlapping_length < min_overlapping_length )
                min_overlapping_length = overlapping_length;
        }
    }
    else
        return -1.0f;
    
    // Case 5:
    // L = ta x uc
    L = normal( crossProduct( ta, uc ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        // Assumes use of netUnitVelocity instead of netVelocity
        if ( overlapping_length < min_overlapping_length )
        {
            overlapping_length /= fabs(netUnitVelocity * L);
            if ( overlapping_length < min_overlapping_length )
                min_overlapping_length = overlapping_length;
        }
    }
    else
        return -1.0f;
    
    // Case 6:
    // L = tb x ua
    L = normal( crossProduct( tb, ua ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        // Assumes use of netUnitVelocity instead of netVelocity
        if ( overlapping_length < min_overlapping_length )
        {
            overlapping_length /= fabs(netUnitVelocity * L);
            if ( overlapping_length < min_overlapping_length )
                min_overlapping_length = overlapping_length;
        }
    }
    else
        return -1.0f;
    
    // Case 7:
    // L = tb x ub
    L = normal( crossProduct( tb, ub ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        // Assumes use of netUnitVelocity instead of netVelocity
        if ( overlapping_length < min_overlapping_length )
        {
            overlapping_length /= fabs(netUnitVelocity * L);
            if ( overlapping_length < min_overlapping_length )
                min_overlapping_length = overlapping_length;
        }
    }
    else
        return -1.0f;
    
    // Case 8:
    // L = tb x uc
    L = normal( crossProduct( tb, uc ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        // Assumes use of netUnitVelocity instead of netVelocity
        if ( overlapping_length < min_overlapping_length )
        {
            overlapping_length /= fabs(netUnitVelocity * L);
            if ( overlapping_length < min_overlapping_length )
                min_overlapping_length = overlapping_length;
        }
    }
    else
        return -1.0f;
    
    // Case 9:
    // L = tc x ua
    L = normal( crossProduct( tc, ua ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        // Assumes use of netUnitVelocity instead of netVelocity
        if ( overlapping_length < min_overlapping_length )
        {
            overlapping_length /= fabs(netUnitVelocity * L);
            if ( overlapping_length < min_overlapping_length )
                min_overlapping_length = overlapping_length;
        }
    }
    else
        return -1.0f;
    
    // Case 10:
    // L = tc x ub
    L = normal( crossProduct( tc, ub ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        // Assumes use of netUnitVelocity instead of netVelocity
        if ( overlapping_length < min_overlapping_length )
        {
            overlapping_length /= fabs(netUnitVelocity * L);
            if ( overlapping_length < min_overlapping_length )
                min_overlapping_length = overlapping_length;
        }
    }
    else
        return -1.0f;
    
    // Case 11:
    // L = tc x uc
    L = normal( crossProduct( tc, uc ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        // Assumes use of netUnitVelocity instead of netVelocity
        if ( overlapping_length < min_overlapping_length )
        {
            overlapping_length /= fabs(netUnitVelocity * L);
            if ( overlapping_length < min_overlapping_length )
                min_overlapping_length = overlapping_length;
        }
    }
    else
        return -1.0f;
    
    return min_overlapping_length / netVelocityMagnitude;
 }
 #endif // #if 0
 
 /**
  * collisionExtent() returns the extent that the triangles need to move in the directions
  * toward their previous positions to avoid the collision; if the extent returned is less
  * than zero, then the two triangles are currently not intersecting. If the two triangles
  * are colliding, but the difference between the specified t_displacement and u_displacement 
  * is the zero vector (i.e. t_displacement == u_displacement), then the t_displacement and 
  * u_displacement will be modified to //the difference between the center of the two triangles
  * or// the negative normal vectors of triangle u and triangle t respectively.
  *
  * @param (const Triangle<TYPENAME>&) t - triangle at its current position
  * @param (const Triangle<TYPENAME>&) u - triangle at its current position
  * @param (Vector3<TYPENAME>&) t_displacement - the current position of t minus the previous position of t
  * @param (Vector3<TYPENAME>&) u_displacement - the current position of u minus the previous position of u
  * @return TYPENAME
  */
 template <typename TYPENAME> 
 inline TYPENAME collisionExtent( const Triangle<TYPENAME>& t, const Triangle<TYPENAME>& u,
                                Vector3<TYPENAME>& t_displacement, Vector3<TYPENAME>& u_displacement )
 {
    // netVelocity = t_displacement - u_displacement
    Vector3<TYPENAME> netVelocity( t_displacement - u_displacement );
    
    const Vector3<TYPENAME>& t_normal( t.getNormal() );
    const Vector3<TYPENAME>& u_normal( u.getNormal() );
    
    // change the displacements if the displacements are equal
    if ( netVelocity.isZeroVector() )
    {
        // Center seems to work better than using normal
        /*t_displacement = u.getCenter() - t.getCenter();
        u_displacement = -t_displacement;
        if ( t_displacement.isZeroVector() )
        {*/
            t_displacement = -u_normal;
            u_displacement = -t_normal;
            
            if ( t_displacement == u_displacement || ( t_displacement * u_displacement ) > ZERO )
            {
                t_displacement.x = 0.0f;
                t_displacement.y = 0.0f;
                t_displacement.z = 0.0f;
                netVelocity = u_displacement; // or -u_displacement, does not matter
            }
            else
                netVelocity = t_displacement - u_displacement;
        /*}
        else
            netVelocity = t_displacement - u_displacement;*/
    }
    
    // min_overlapping_length in the direction of the net velocity
    // overlapping_length / | separating_axis * netVelocity |
    TYPENAME min_overlapping_length( std::numeric_limits<float>::max() );
    
    // check if it is possible for t and u to be coplanar
    if ( t_normal == u_normal )
    {
        // if t and u are coplanar
        Vector3<TYPENAME> cp( crossProduct( crossProduct( u.getEndPointA() - t.getEndPointA(), t.getEndPointB() - t.getEndPointA() ), t_normal ) );
        
        // assume t and u are coplanar if cp is approximately (0, 0, 0)
        if ( (cp.x > -EPSILON || cp.x < EPSILON) 
                        && (cp.y > -EPSILON || cp.y < EPSILON) 
                        && (cp.z > -EPSILON || cp.z < EPSILON) )
        {
            // Assume we are now performing collision detection in the 2D plane
            // There are 6 separating axes, and they are perpendicular to each of the edges
            
            Vector3<TYPENAME> L( normal( crossProduct( t_normal, t.getEndPointB() - t.getEndPointA() ) ) );
            TYPENAME overlapping_length( collisionExtent( t, u, t_displacement, u_displacement, L ) );
            
            // Case 1:
            // L = t.normal x (t.b - t.a)
            if ( overlapping_length >= 0.0f )
            {
                overlapping_length /= fabs(netVelocity * L);
                //if ( overlapping_length < min_overlapping_length )
                    min_overlapping_length = overlapping_length;
            }
            else
                return -1.0f;
            
            // Case 2:
            // L = t.normal x (t.c - t.b)
            L = normal( crossProduct( t_normal, t.getEndPointC() - t.getEndPointB() ) );
            overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
            if ( overlapping_length >= 0.0f )
            {
                overlapping_length /= fabs(netVelocity * L);
                if ( overlapping_length < min_overlapping_length )
                    min_overlapping_length = overlapping_length;
            }
            else
                return -1.0f;
            
            // Case 3:
            // L = t.normal x (t.a - t.c)
            L = normal( crossProduct( t_normal, t.getEndPointA() - t.getEndPointC() ) );
            overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
            if ( overlapping_length >= 0.0f )
            {
                overlapping_length /= fabs(netVelocity * L);
                if ( overlapping_length < min_overlapping_length )
                    min_overlapping_length = overlapping_length;
            }
            else
                return -1.0f;
            
            // Case 4:
            // L = u.normal x (u.b - u.a)
            L = normal( crossProduct( u_normal, u.getEndPointB() - u.getEndPointA() ) );
            overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
            if ( overlapping_length >= 0.0f )
            {
                overlapping_length /= fabs(netVelocity * L);
                if ( overlapping_length < min_overlapping_length )
                    min_overlapping_length = overlapping_length;
            }
            else
                return -1.0f;
            
            // Case 5:
            // L = u.normal x (u.c - u.b)
            L = normal( crossProduct( u_normal, u.getEndPointC() - u.getEndPointB() ) );
            overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
            if ( overlapping_length >= 0.0f )
            {
                overlapping_length /= fabs(netVelocity * L);
                if ( overlapping_length < min_overlapping_length )
                    min_overlapping_length = overlapping_length;
            }
            else
                return -1.0f;
            
            // Case 6:
            // L = u.normal x (u.a - u.c)
            L = normal( crossProduct( u_normal, u.getEndPointA() - u.getEndPointC() ) );
            overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
            if ( overlapping_length >= 0.0f )
            {
                overlapping_length /= fabs(netVelocity * L);
                if ( overlapping_length < min_overlapping_length )
                    min_overlapping_length = overlapping_length;
            }
            else
                return -1.0f;
            
            return min_overlapping_length;
        }
    }
    
    Vector3<TYPENAME> ta( t.getEndPointB() - t.getEndPointA() );
    Vector3<TYPENAME> tb( t.getEndPointC() - t.getEndPointB() );
    Vector3<TYPENAME> tc( t.getEndPointA() - t.getEndPointC() );
    
    Vector3<TYPENAME> ua( u.getEndPointB() - u.getEndPointA() );
    Vector3<TYPENAME> ub( u.getEndPointC() - u.getEndPointB() );
    Vector3<TYPENAME> uc( u.getEndPointA() - u.getEndPointC() );
    
    TYPENAME overlapping_length( collisionExtent( t, u, t_displacement, u_displacement, t_normal ) );
    
    // Case 1:
    // L = normal( t )
    if ( overlapping_length >= 0.0f )
    {
        overlapping_length /= fabs(netVelocity * t_normal);
        //if ( overlapping_length < min_overlapping_length )
            min_overlapping_length = overlapping_length;
    }
    else
        return -1.0f;
    
    // Case 2:
    // L = normal( u )
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, u_normal );
    if ( overlapping_length >= 0.0f )
    {
        overlapping_length /= fabs(netVelocity * u_normal);
        if ( overlapping_length < min_overlapping_length )
            min_overlapping_length = overlapping_length;
    }
    else
        return -1.0f;
    
    // Case 3:
    // L = ta x ua
    Vector3<TYPENAME> L( normal( crossProduct( ta, ua ) ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        overlapping_length /= fabs(netVelocity * L);
        if ( overlapping_length < min_overlapping_length )
            min_overlapping_length = overlapping_length;
    }
    else
        return -1.0f;
    
    // Case 4:
    // L = ta x ub
    L = normal( crossProduct( ta, ub ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        overlapping_length /= fabs(netVelocity * L);
        if ( overlapping_length < min_overlapping_length )
            min_overlapping_length = overlapping_length;
    }
    else
        return -1.0f;
    
    // Case 5:
    // L = ta x uc
    L = normal( crossProduct( ta, uc ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        overlapping_length /= fabs(netVelocity * L);
        if ( overlapping_length < min_overlapping_length )
            min_overlapping_length = overlapping_length;
    }
    else
        return -1.0f;
    
    // Case 6:
    // L = tb x ua
    L = normal( crossProduct( tb, ua ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        overlapping_length /= fabs(netVelocity * L);
        if ( overlapping_length < min_overlapping_length )
            min_overlapping_length = overlapping_length;
    }
    else
        return -1.0f;
    
    // Case 7:
    // L = tb x ub
    L = normal( crossProduct( tb, ub ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        overlapping_length /= fabs(netVelocity * L);
        if ( overlapping_length < min_overlapping_length )
            min_overlapping_length = overlapping_length;
    }
    else
        return -1.0f;
    
    // Case 8:
    // L = tb x uc
    L = normal( crossProduct( tb, uc ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        overlapping_length /= fabs(netVelocity * L);
        if ( overlapping_length < min_overlapping_length )
            min_overlapping_length = overlapping_length;
    }
    else
        return -1.0f;
    
    // Case 9:
    // L = tc x ua
    L = normal( crossProduct( tc, ua ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        overlapping_length /= fabs(netVelocity * L);
        if ( overlapping_length < min_overlapping_length )
            min_overlapping_length = overlapping_length;
    }
    else
        return -1.0f;
    
    // Case 10:
    // L = tc x ub
    L = normal( crossProduct( tc, ub ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        overlapping_length /= fabs(netVelocity * L);
        if ( overlapping_length < min_overlapping_length )
            min_overlapping_length = overlapping_length;
    }
    else
        return -1.0f;
    
    // Case 11:
    // L = tc x uc
    L = normal( crossProduct( tc, uc ) );
    overlapping_length = collisionExtent( t, u, t_displacement, u_displacement, L );
    if ( overlapping_length >= 0.0f )
    {
        overlapping_length /= fabs(netVelocity * L);
        if ( overlapping_length < min_overlapping_length )
            min_overlapping_length = overlapping_length;
    }
    else
        return -1.0f;
    
    return min_overlapping_length;
 }
 
 /**
  * collisionExtent() returns the extent the projection of the two specified triangles onto the
  * specified axis intersect. The two specified triangles do not intersect if the entent is less than zero. 
  * The axis does not have to be a unit vector.
  *
  * @param (const Triangle<TYPENAME>&) t - triangle at its current position
  * @param (const Triangle<TYPENAME>&) u - triangle at its current position
  * @param (const Vector3<TYPENAME>&) t_displacement - the current position of t minus the previous position of t
  * @param (const Vector3<TYPENAME>&) u_displacement - the current position of u minus the previous position of u
  * @param (const Vector3<TYPENAME>&) axis
  * @return bool
  */
 template <typename TYPENAME> 
 inline TYPENAME collisionExtent( const Triangle<TYPENAME>& t, const Triangle<TYPENAME>& u, 
             const Vector3<TYPENAME>& t_displacement, const Vector3<TYPENAME>& u_displacement, const Vector3<TYPENAME>& axis )
 {
    // for testing-only
    if ( !collideSeparatingAxis( t, u, axis ) )
        return -ONE;
 
    TYPENAME t_prev_extent_A( (t.getEndPointA() - t_displacement) * axis );
    TYPENAME t_prev_extent_B( (t.getEndPointB() - t_displacement) * axis );
    TYPENAME t_prev_extent_C( (t.getEndPointC() - t_displacement) * axis );
    
    TYPENAME u_prev_extent_A( (u.getEndPointA() - u_displacement) * axis );
    TYPENAME u_prev_extent_B( (u.getEndPointB() - u_displacement) * axis );
    TYPENAME u_prev_extent_C( (u.getEndPointC() - u_displacement) * axis );
    
    TYPENAME t_prev_min( ( t_prev_extent_A < t_prev_extent_B ? 
                                ( t_prev_extent_A < t_prev_extent_C ? t_prev_extent_A : t_prev_extent_C ) : 
                                ( t_prev_extent_B < t_prev_extent_C ? t_prev_extent_B : t_prev_extent_C ) ) );
    TYPENAME u_prev_min( ( u_prev_extent_A < u_prev_extent_B ? 
                                ( u_prev_extent_A < u_prev_extent_C ? u_prev_extent_A : u_prev_extent_C ) : 
                                ( u_prev_extent_B < u_prev_extent_C ? u_prev_extent_B : u_prev_extent_C ) ) );
    
    TYPENAME t_extent_A( t.getEndPointA() * axis );
    TYPENAME t_extent_B( t.getEndPointB() * axis );
    TYPENAME t_extent_C( t.getEndPointC() * axis );
    
    TYPENAME u_extent_A( u.getEndPointA() * axis );
    TYPENAME u_extent_B( u.getEndPointB() * axis );
    TYPENAME u_extent_C( u.getEndPointC() * axis );
    
    if ( u_prev_min < t_prev_min )
    {
        // if triangle u is moving deeper into triangle t in the negative direction of the axis
        if ( u_displacement * axis < ZERO )
        {
            TYPENAME u_min( ( u_extent_A < u_extent_B ? ( u_extent_A < u_extent_C ? u_extent_A : u_extent_C ) : 
                                                      ( u_extent_B < u_extent_C ? u_extent_B : u_extent_C ) ) );
            TYPENAME t_max( t_extent_A > t_extent_B ? ( t_extent_A > t_extent_C ? t_extent_A : t_extent_C ) : 
                                                        ( t_extent_B > t_extent_C ? t_extent_B : t_extent_C ) );
            
            // Notice we have to accommodate the fact that triangle t may move in the negative direction of the axis
            // at a faster rate than triangle u
            if ( t_max > u_min )
                // return t_max - u_min if t_max > u_min
                return t_max - u_min;
            else if ( t_max < u_min )
                // return -1 because there is no intersection
                return -ONE;
            else // t_max == u_min
                // return 0 because the intersection is at exactly 1 point
                return ZERO;
        }
        // if triangle u is either moving deeper into triangle t in the positive direction of the axis or not moving at all
        else // u_displacement * axis >= ZERO
        {
            TYPENAME t_min( ( t_extent_A < t_extent_B ? ( t_extent_A < t_extent_C ? t_extent_A : t_extent_C ) : 
                                                      ( t_extent_B < t_extent_C ? t_extent_B : t_extent_C ) ) );
            TYPENAME u_max( u_extent_A > u_extent_B ? ( u_extent_A > u_extent_C ? u_extent_A : u_extent_C ) : 
                                                        ( u_extent_B > u_extent_C ? u_extent_B : u_extent_C ) );
            
            if ( u_max > t_min )
                // return u_max - t_min if u_prev_min is before t_prev_min, and u_max > t_min
                return u_max - t_min;
            else if ( u_max < t_min )
                // return -1 because there is no intersection
                return -ONE;
            else // u_max == t_min
                // return 0 because the intersection is at exactly 1 point
                return ZERO;
        }
    }
    else // t_prev_min <= u_prev_min
    {
        // if triangle t is moving deeper into triangle u in the negative direction of the axis
        if ( t_displacement * axis < ZERO )
        {
            TYPENAME t_min( ( t_extent_A < t_extent_B ? ( t_extent_A < t_extent_C ? t_extent_A : t_extent_C ) : 
                                                      ( t_extent_B < t_extent_C ? t_extent_B : t_extent_C ) ) );
            TYPENAME u_max( u_extent_A > u_extent_B ? ( u_extent_A > u_extent_C ? u_extent_A : u_extent_C ) : 
                                                        ( u_extent_B > u_extent_C ? u_extent_B : u_extent_C ) );
            
            // Notice we have to accommodate the fact that triangle u may move in the negative direction of the axis
            // at a faster rate than triangle t
            if ( u_max > t_min )
                // return u_max - t_min if u_max > t_min
                return u_max - t_min;
            else if ( u_max < t_min )
                // return -1 because there is no intersection
                return -ONE;
            else // u_max == t_min
                // return 0 because the intersection is at exactly 1 point
                return ZERO;
        }
        // if triangle t is either moving deeper into triangle u in the positive direction of the axis or not moving at all
        else // t_displacement * axis >= ZERO
        {
            TYPENAME u_min( ( u_extent_A < u_extent_B ? ( u_extent_A < u_extent_C ? u_extent_A : u_extent_C ) : 
                                                      ( u_extent_B < u_extent_C ? u_extent_B : u_extent_C ) ) );
            TYPENAME t_max( t_extent_A > t_extent_B ? ( t_extent_A > t_extent_C ? t_extent_A : t_extent_C ) : 
                                                        ( t_extent_B > t_extent_C ? t_extent_B : t_extent_C ) );
            
            if ( t_max > u_min )
                // return t_max - u_min if t_prev_min is before u_prev_min, and t_max > u_min
                return t_max - u_min;
            else if ( t_max < u_min )
                // return -1 because there is no intersection
                return -ONE;
            else // t_max == u_min
                // return 0 because the intersection is at exactly 1 point
                return ZERO;
        }
    }
 }
 
 #if 0
 /**
  * interpolateCollision() returns the amount of time that needs to pass before the two specified 
  * OBBs collide, assuming we already know that the two OBBs will collide, and they moved back to 
  * their previous positions before the collision; the return time value is between 0 and 1 (inclusive).
  *
  * @param (OBB<TYPENAME>&) obbA
  * @param (OBB<TYPENAME>&) obbB
  * @param (const Vector3<TYPENAME>&) netVelocity
  * @return TYPENAME
  */
 template< typename TYPENAME >
 inline bool interpolateCollision( OBB<TYPENAME>& obbA, OBB<TYPENAME>& obbB, const Vector3<TYPENAME>& netVelocity )
 {
 
 #if GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    if ( !obbA.isMaintained )
        maintain( obbA );
    if ( !obbB.isMaintained )
        maintain( obbB );
 #endif // GL_OBJECT_COLLISION_DETECTION_TYPE == 1
    
    // T = PB – PA
    Vector3<TYPENAME> T( obbB.center - obbA.center );
    
    // The axes for obbA
    Vector3<TYPENAME> Ax = obbA.orientation.getXAxis();
    Vector3<TYPENAME> Ay = obbA.orientation.getYAxis();
    Vector3<TYPENAME> Az = obbA.orientation.getZAxis();
    
    // The axes for obbB
    Vector3<TYPENAME> Bx = obbB.orientation.getXAxis();
    Vector3<TYPENAME> By = obbB.orientation.getYAxis();
    Vector3<TYPENAME> Bz = obbB.orientation.getZAxis();
    
    // Rij = Ai • Bj 
    OrientationMatrix3<TYPENAME> R = obbA.orientation * obbB.orientation;
    
    const TYPENAME& WA = obbA.half_width;
    const TYPENAME& HA = obbA.half_height;
    const TYPENAME& DA = obbA.half_depth;
    const TYPENAME& WB = obbB.half_width;
    const TYPENAME& HB = obbB.half_height;
    const TYPENAME& DB = obbB.half_depth;
    
    // The 6 separating planes for the faces of the two boxes
    
    TYPENAME separating_length( fabs( T*Ax ) - WA + fabs( WB*R.Xx ) + fabs( HB*R.Xy ) + fabs( DB*R.Xz ) );
    // max_separating_magnitude is the separating magnitude/length in the direction of the netVelocity
    TYPENAME max_separating_magnitude( 0.0f ); // separating_length / (separating_axis * netVelocity)
    TYPENAME temp;
    
    // Case 1:
    // L = Ax
    // | T*Ax | > WA + | WB * Rxx | + | HB * Rxy | + | DB * Rxz |
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(Ax * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 2:
    // L = Ay
    // | T*Ay | > HA + | WB * Ryx | + | HB * Ryy | + | DB * Ryz |
    separating_length = fabs( T*Ay ) - HA + fabs( WB*R.Yx ) + fabs( HB*R.Yy ) + fabs( DB*R.Yz );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(Ay * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 3:
    // L = Az
    // | T*Az | > DA + | WB * Rzx | + | HB * Rzy | + | DB * Rzz |
    separating_length = fabs( T*Az ) - DA + fabs( WB*R.Zx ) + fabs( HB*R.Zy ) + fabs( DB*R.Zz );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(Az * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 4:
    // L = Bx
    // | T*Bx | > WB + | WA * Rxx | + | HA * Ryx | + | DA * Rzx |
    separating_length = fabs( T*Bx ) - WB + fabs( WA*R.Xx ) + fabs( HA*R.Yx ) + fabs( DA*R.Zx );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(Bx * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 5:
    // L = By
    // | T*By | > HB + | WA * Rxy | + | HA * Ryy | + | DA * Rzy |
    separating_length = fabs( T*By ) - HB + fabs( WA*R.Xy ) + fabs( HA*R.Yy ) + fabs( DA*R.Zy );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(By * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 6:
    // L = Bz
    // | T*Bz | > DB + | WA * Rxz | + | HA * Ryz | + | DA * Rzz |
    separating_length = fabs( T*Bz ) - DB + fabs( WA*R.Xz ) + fabs( HA*R.Yz ) + fabs( DA*R.Zz );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(Bz * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // The 9 separating planes for the edges of the two boxes
    
    // Case 7:
    // L = Ax x Bx
    // |(T*Az)*Ryx - (T*Ay)*Rzx| > |HA*Rzx| + |DA*Ryx| + |HB*Rxz| + |DB*Rxy|
    separating_length = fabs( (T*Az*R.Yx) - (T*Ay*R.Zx) )
                            - fabs( HA*R.Zx ) + fabs( DA*R.Yx ) + fabs( HB*R.Xz ) + fabs( DB*R.Xy );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(normal( crossProduct( Ax, Bx ) ) * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 8:
    // L = Ax x By
    // |(T*Az)*Ryy - (T*Ay)*Rzy| > |HA*Rzy| + |DA*Ryy| + |WB*Rxz| + |DB*Rxx|
    separating_length = fabs( (T*Az*R.Yy) - (T*Ay*R.Zy) ) 
                            - fabs( HA*R.Zy ) + fabs( DA*R.Yy ) + fabs( WB*R.Xz ) + fabs( DB*R.Xx );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(normal( crossProduct( Ax, By ) ) * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 9:
    // L = Ax x Bz
    // |(T*Az)*Ryz - (T*Ay)*Rzz| > |HA*Rzz| + |DA*Ryz| + |WB*Rxy| + |HB*Rxx|
    separating_length = fabs( (T*Az*R.Yz) - (T*Ay*R.Zz) ) 
                            - fabs( HA*R.Zz ) + fabs( DA*R.Yz ) + fabs( WB*R.Xy ) + fabs( HB*R.Xx );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(normal( crossProduct( Ax, Bz ) ) * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 10:
    // L = Ay x Bx
    // |(T*Ax)*Rzx - (T*Az)*Rxx| > |WA*Rzx| + |DA*Rxx| + |HB*Ryz| + |DB*Ryy|
    separating_length = fabs( (T*Ax*R.Zx) - (T*Az*R.Xx) )
                            - fabs( WA*R.Zx ) + fabs( DA*R.Xx ) + fabs( HB*R.Yz ) + fabs( DB*R.Yy );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(normal( crossProduct( Ay, Bx ) ) * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 11:
    // L = Ay x By
    // |(T*Ax)*Rzy - (T*Az)*Rxy| > |WA*Rzy| + |DA*Rxy| + |WB*Ryz| + |DB*Ryx|
    separating_length = fabs( (T*Ax*R.Zy) - (T*Az*R.Xy) ) 
                            - fabs( WA*R.Zy ) + fabs( DA*R.Xy ) + fabs( WB*R.Yz ) + fabs( DB*R.Yx );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(normal( crossProduct( Ay, By ) ) * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 12:
    // L = Ay x Bz
    // |(T*Ax)*Rzz - (T*Az)*Rxz| > |WA*Rzz| + |DA*Rxz| + |WB*Ryy| + |HB*Ryx|
    separating_length = fabs( (T*Ax*R.Zz) - (T*Az*R.Xz) ) 
                            - fabs( WA*R.Zz ) + fabs( DA*R.Xz ) + fabs( WB*R.Yy ) + fabs( HB*R.Yx );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(normal( crossProduct( Ay, Bz ) ) * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 13:
    // L = Az x Bx
    // |(T*Ay)*Rxx - (T*Ax)*Ryx| > |WA*Ryx| + |HA*Rxx| + |HB*Rzz| + |DB*Rzy|
    separating_length = fabs( (T*Ay*R.Xx) - (T*Ax*R.Yx) ) 
                            - fabs( WA*R.Yx ) + fabs( HA*R.Xx ) + fabs( HB*R.Zz ) + fabs( DB*R.Zy );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(normal( crossProduct( Az, Bx ) ) * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 14:
    // L = Az x By
    // |(T*Ay)*Rxy - (T*Ax)*Ryy| > |WA*Ryy| + |HA*Rxy| + |WB*Rzz| + |DB*Rzx|
    separating_length = fabs( (T*Ay*R.Xy) - (T*Ax*R.Yy) ) 
                            - fabs( WA*R.Yy ) + fabs( HA*R.Xy ) + fabs( WB*R.Zz ) + fabs( DB*R.Zx );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(normal( crossProduct( Az, By ) ) * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // Case 15:
    // L = Az x Bz
    // |(T*Ay)*Rxz - (T*Ax)*Ryz| > |WA*Ryz| + |HA*Rxz| + |WB*Rzy| + |HB*Rzx|
    separating_length = fabs( (T*Ay*R.Xz) - (T*Ax*R.Yz) )
                            - fabs( WA*R.Yz ) + fabs( HA*R.Xz ) + fabs( WB*R.Zy ) + fabs( HB*R.Zx );
    if ( separating_length > 0.0f )
    {
        temp = separating_length / fabs(normal( crossProduct( Az, Bz ) ) * netVelocity);
        if ( temp > max_separating_magnitude )
            max_separating_magnitude = temp;
    }
    
    // separating_time = max_separating_magnitude / netVelocityMagnitude 
    return max_separating_magnitude / magnitude( netVelocity );
 }
 #endif // #if 0
 #endif // IMPLEMENT_INTERPOLATION == 1
 
 /**
  * findMinMaxProjectionExtent() adjusts the specified min and max to reflect the projection 
  * of the endpoints of the specified triangle onto the specified axis if the projection 
  * value is less than min or greater than max respectively.
  *
  * @param (const Triangle<TYPENAME>&) triangle
  * @param (const Vector3<TYPENAME>&) axis
  * @param (TYPENAME&) min
  * @param (TYPENAME&) max
  */
 /*template <typename TYPENAME>
 inline void findMinMaxProjectionExtent( const Triangle<TYPENAME>& triangle, const Vector3<TYPENAME>& axis, 
                                         TYPENAME& min, TYPENAME& max )
 {
    findMinMaxProjectionExtent( triangle.getEndPointA(), axis, min, max );
    findMinMaxProjectionExtent( triangle.getEndPointB(), axis, min, max );
    findMinMaxProjectionExtent( triangle.getEndPointC(), axis, min, max );
 }*/
 
 /**
  * PointTriangleIntersects() returns true if the point intersects the triangle; otherwise, false is returned.
  *
  * @param (const Vector3<TYPENAME>&) point
  * @param (const Triangle<TYPENAME>&) triangle
  * @param (const TYPENAME&) triangleArea - the area of the triangle for optimization purposes
  */
 template <typename TYPENAME> 
 inline bool PointTriangleIntersects( const Vector3<TYPENAME>& point, 
                                      const Triangle<TYPENAME>& triangle, const TYPENAME& triangleArea )
 {
    
    // crossProduct( u, v ) = vector, whose magnitude is the area of the parallelogram formed by vectors u and v.
    TYPENAME areaAB = magnitude( crossProduct(triangle.a - point, triangle.b - point) ) / 2.0f;
    TYPENAME areaBC = magnitude( crossProduct(triangle.b - point, triangle.c - point) ) / 2.0f;
    TYPENAME areaCA = magnitude( crossProduct(triangle.c - point, triangle.a - point) ) / 2.0f;
        
    TYPENAME areaSum = areaAB + areaBC + areaCA;
    
    // if the calculated area is equal to the triangle's area, then the point lies in the triangle
    if ( areaSum <= triangleArea ) //+ EPSILON )    // && areaSum >= triangleArea - epsilon
        return true;
    else
        return false;
    
    // Currently using area method (27 multiplications + 3 square roots + 3 divisions)
    // OR
    // Barycentric coordinates method (37 multiplications + 1 division)
    // P = test point
    // A, B, C = points defining the triangle
    // L1, L2, L3 = Barycentric coordinates constants
    //
    // Px = (L1)Ax + (L2)Bx + (L3)Cx
    // Py = (L1)Ay + (L2)By + (L3)Cy
    // Pz = (L1)Az + (L2)Bz + (L3)Cz
    // if ( L1 + L2 + L3 >= 0 && L1 + L2 + L3 <= 1 ), then point P is within the triangle's boundary
    //
    // [Px   [Ax Bx Cx    [L1
    //  Py =  Ay By Cy  *  L2
    //  Pz]   Az Bz Cz]    L3]
    //
    // P = M * L
    // L = (M^-1) * P
    //
    // Or try...Nevermind...too many conditions
    //
    // L1 = (Px - [((Pz - Cz)(Bx - Cx))/(Bz - Cz)] - Cx) / ((Ax - Cx) - ((Az - Cz)(Bx - Cx)))
    // L2 = (Pz - Cz - L1(Az - Cz)) / (Bz - Cz)
    // L3 = (Pz - L1*Az - L2*Bz) / Cz
    /*Vector3<TYPENAME> L = inverse( Matrix3<TYPENAME>( triangle.a, triangle.b, triangle.c ) ) * point;
    
    TYPENAME sum = L.x + L.y + L.z;
    
    if ( sum >= 1.0f - EPSILON
        && sum <= 1.0f + EPSILON )  // accommodate floating-point error
        return true;
    else
        return false;*/
 }
 
 /**
  * LineTriangleIntersects() returns true if the line intersects the triangle; otherwise, false is returned.
  *
  * @param (const Vector3<TYPENAME>&) lineOrigin - the starting point of the line
  * @param (const Vector3<TYPENAME>&) line - the direction and magnitude of the line from the lineOrigin
  * @param (const Triangle<TYPENAME>&) triangle
  * @param (const Triangle<TYPENAME>&) triangleArea - the area of the triangle for optimization purposes
  * @return bool
  */
 template <typename TYPENAME>
 inline bool LineTriangleIntersects( const Vector3<TYPENAME>& lineOrigin, const Vector3<TYPENAME>& line, 
                                        const Triangle<TYPENAME>& triangle, const TYPENAME& triangleArea )
 {
    Vector3<TYPENAME> lineDirection = normal(line);
    
    Vector3<TYPENAME> planeNormal = crossProduct((triangle.b - triangle.a), (triangle.c - triangle.a));
    TYPENAME distanceToPlaneIntersection = getRayPlaneIntersectionDistance( lineOrigin, lineDirection, 
                                                                            planeNormal, triangle.a );
    /*printf("%1f Test\n", distanceToPlaneIntersection);
    print( lineOrigin );
    print( triangle.a );
    print( triangle.b );
    print( triangle.c );*/
    //print( triangle ); // 7, -1, 7.5
    //printf( "Intersect? %1d\n", PointTriangleIntersects( Vector3<TYPENAME>( 7.0f, -1.0f, 7.5f ), triangle, triangleArea ) );
    // 1) Check if distanceToPlaneIntersection >= 0 and distanceToPlaneIntersection <= length( line ); if not, return false.
    if ( distanceToPlaneIntersection < 0.0f 
            || distanceToPlaneIntersection*distanceToPlaneIntersection > magnitude_squared( line )  )
        return false;
    // 2) Check if (lineOrigin + distanceToPlaneIntersection*lineDirection) is inside the triangle
    else
    {
        Vector3<TYPENAME> p = lineOrigin + (distanceToPlaneIntersection*lineDirection);   // line-plane intersection point
        return PointTriangleIntersects( p, triangle, triangleArea );
    }
 }
 
 /**
  * RayTriangleIntersects() returns true if the ray intersects the triangle; otherwise, false is returned.
  *
  * @param (const Vector3<TYPENAME>&) rayOrigin - the starting point of the ray
  * @param (const Vector3<TYPENAME>&) rayDirection - the direction of the ray from the rayOrigin
  * @param (const Triangle<TYPENAME>&) triangle
  * @param (const Triangle<TYPENAME>&) triangleArea - the area of the triangle for optimization purposes
  * @return bool
  */
 template <typename TYPENAME>
 inline bool RayTriangleIntersects( const Vector3<TYPENAME>& rayOrigin, const Vector3<TYPENAME>& rayDirection, 
                                        const Triangle<TYPENAME>& triangle, const TYPENAME& triangleArea )
 {
    Vector3<TYPENAME> planeNormal = crossProduct((triangle.b - triangle.a), (triangle.c - triangle.a));
    TYPENAME distanceToPlaneIntersection = getRayPlaneIntersectionDistance( rayOrigin, rayDirection, 
                                                                            planeNormal, triangle.a );
    
    // 1) Check if distanceToPlaneIntersection >= 0; if not, return false.
    if ( distanceToPlaneIntersection < 0.0f )
        return false;
    // 2) Check if (rayOrigin + distanceToPlaneIntersection*rayDirection) is inside the triangle
    else
    {
        Vector3<TYPENAME> p = lineOrigin + (distanceToPlaneIntersection*lineDirection);   // line-plane intersection point
        return PointTriangleIntersects( p, triangle, triangleArea );
    }
 }
 
 /**
  * print() prints out the 3 positions that define the triangle.
  *
  * @param (const Triangle<TYPENAME>&) t
  */
 template<typename TYPENAME>
 inline void print( const Triangle<TYPENAME>& t )
 {
    printf( "Triangle:\na: " );
    print(t.a);
    printf( "b: " );
    print(t.b);
    printf( "c: " );
    print(t.c);
 }
 
 /**
  * render() contains the code necessary to render a triangle in OpenGL.
  *
  * @param (const Triangle<TYPENAME>&) t
  */
 template <typename TYPENAME>
 inline void render( const Triangle<TYPENAME>& t )
 {
    Render_Shape::Triangle_Solid( t.a, t.b, t.c );
 }

 #endif // TRIANGLE_H